#include <motion_controller/motion_controller_node.h>

MotionController::MotionController(tf2_ros::Buffer* tf_buffer){
    // 参数配置
    __robot_radius = 0.3;
    
    // 路径规划器
    __planner_bridge_ptr = boost::make_shared<PlannerBridge>(tf_buffer);
    __sensor_bridge_ptr = boost::make_shared<SensorBridge>(tf_buffer);
    // 规控器
    __controller_ptr = boost::make_shared<ControllerP2P>(__planner_bridge_ptr, __sensor_bridge_ptr);
   
    // ROS 
    __rviz_pose_sub = __nh.subscribe("/move_base_simple/goal", 10, &MotionController::RvizPosesCallback, this);
    __map_sub = __nh.subscribe("/map", 10, &MotionController::MapCallback, this);

    
    // // 状态机测试
    // __controller_ptr->SetEvent(ControllerState::Event::EV_WORKING);
    // for(int i=0; i<20; i++){
    //     LOG(INFO) << "--------------------------------";
    //     cout << "[" << i << "] curr_state is " << __controller_ptr->GetCurrStateName() << endl;
    //     if(i==6){
    //         __controller_ptr->SetEvent(ControllerState::Event::EV_CANCEL);
    //     }
    //     if(i==15){
    //         __controller_ptr->SetEvent(ControllerState::Event::EV_WORKING);
    //     }
    //     __controller_ptr->HandleEvent();
    // }

    Loop();
}

MotionController::~MotionController(){
    
}






// 循环控制
void MotionController::Loop(){
    __controller_ptr->SetEvent(ControllerState::Event::EV_IDLE);
    ros::Rate controller_rate(20);  //20Hz
    while(!ros::isShuttingDown()){
        controller_rate.sleep();
        ros::spinOnce();
        LOG(INFO) << "--------------------------------";

        // 阻塞状态检测
        // 获取机器人当前位姿
        geometry_msgs::PoseStamped global_robot_pose;
        if(!__planner_bridge_ptr->GetRobotPose(global_robot_pose)){
            LOG(WARNING) << "can not get robot pose!";
            // 进入阻塞状态  待补全
        }

        // 状态机控制
        ROS_INFO_STREAM("curr_state is " << __controller_ptr->GetCurrStateName());
        __controller_ptr->HandleEvent(global_robot_pose);
    }
}
